/***************************************************************************\
 * Copyright (C) by Keio University
 * BilateralFilter.cpp created in 11 2011.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * This file is part of HVRL Engine Project.
 *
 * HVRL Engine Project is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * HVRL Engine Project is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
\***************************************************************************/


/*
Based on the exampls from the nVidia SDK's example BilateralFilter
Based on the separable kernel optimization
*/

#include <cmath>
#include <cstring>
#include <iostream>


extern "C" void computeGaussianGold(float delta, int radius);
extern "C" void bilateralFilterDepthGold(unsigned char* pSrc,
        unsigned char* pDest,
        float e_d,
        int w, int h, int r);
//variables
float gaussian[50];

void computeGaussianGold(float delta, int radius)
{
    for(int i = 0; i < 2 * radius + 1; i++)
    {
        int x = i - radius;
        gaussian[i] = exp(-(x * x) /
                          (2 * delta * delta));
    }
}

void bilateralFilterDepthGold(unsigned char* pSrc,
                              unsigned char* pDest,
                              float e_d,
                              int w, int h, int r)
{
    float domainDist, colorDist, factor;

    for(int y = 0; y < h; y++)
    {
        for(int x = 0; x < w; x++)
        {
            float t = 0.0f;
            float sum = 0.0f;
            float div = 1.0f /(2.0f*e_d*e_d);
            for(int i = -r; i <= r; i++)
            {
                int neighborY = y + i;

                if(neighborY> 0 && neighborY<h)
                {
                    for(int j = -r; j <= r; j++)
                    {
                        domainDist = gaussian[r + i] * gaussian[r + j];
                        int neighborX = x + j;
                        if(neighborY> 0 && neighborY<h)
                        {

                        float diff = float(pSrc[y * w + x]-pSrc[neighborY * w + neighborX])* 0.003921568627f;
                        colorDist = exp(-diff*diff*div);
                        factor = domainDist * colorDist;

                        sum += factor;
                        t += factor*float(pSrc[neighborY * w + neighborX])* 0.003921568627f;

                        }
                    }
                }
            }
            pDest[y * w + x] = (unsigned char) (t / sum * 255.0f);
        }
    }
}
